#include "SplineR2.hpp"

SplineR2::SplineR2( const std::vector<Spline::Point>& control_points, const Parametrization& parametrization ) 
{
	uint k = 0;

	foreach( point, control_points )
	{
		double t = parametrization( k );
		_cp_x.push_back( Spline::Point( t, point->x ) );
		_cp_y.push_back( Spline::Point( t, point->y ) );
		k++;
	}

	_sx = new Spline( _cp_x );
	_sy = new Spline( _cp_y );
}

SplineR2::~SplineR2()
{
	delete _sx;
	delete _sy;
}

Spline::Point SplineR2::operator() ( double t ) const
{
	return eval( t );
}

Spline::Point SplineR2::eval( double t ) const
{
	return Spline::Point( (*_sx)(t), (*_sy)(t) );
}

std::vector<Spline::Point> SplineR2::sample( uint samples ) const
{
	std::vector<Spline::Point> points;
	double step = 1.0f/(samples-1);
	for( uint s = 0 ; s < samples ; s++ )
		points.push_back( eval( s*step ) );
	return points;
}

double SplineR2::distance( const Spline::Point& p, double t, bool squared ) const
{
	double dx = (p.x-(*_sx)(t));
	double dy = (p.y-(*_sy)(t));
	double dist_sq = dx*dx + dy*dy;
	if( !squared )
		return std::sqrt( dist_sq );
	return dist_sq;
}

double SplineR2::distanceDerivative( const Spline::Point& p, double t ) const
{
	return -(p.x-(*_sx)(t))*_sx->derivative(t) - (p.y-(*_sy)(t))*_sx->derivative(t);
}

double SplineR2::nearest( const Spline::Point& p ) const
{
	double t_nearest, sq_distance;
	forsn( i, 1, _cp_x.size()-1 )
	{
		// obtiene el t del punto más cercano del polinomio i-1, 
		// y su distancia al cuadrado
		double sqd;
		double t = nearest_point( p, i-1, sqd );

		// se queda con la menor entre la actual y la menor 
		// hasta el momento
		if( i==1 || sq_distance > sqd )
		{
			t_nearest   = t;
			sq_distance = sqd;
		}
	}
	return t_nearest;
}

double SplineR2::zeroWithBisection( const Spline::Point& point, double lbound, double rbound ) const
{
	const uint N = 50;

	double t0  = lbound;
	double dt0 = distanceDerivative( point, t0 );
	double t;

	forno( iteration, N )
	{
		t = (t0+rbound)/2;

		// calcula la derivada de la distancia en el punto
		double dt = distanceDerivative( point, t );

		// achica el intervalo por el lado izquierdo si mantiene el signo
		// si no, achica por el lado derecho
		if( (dt<0 && dt0<0) || (dt>0 && dt0>0) )
		{
			t0  = t;
			dt0 = dt;
		}
		else
			rbound = t;
	}

	return t;
}

double SplineR2::zeroWithNewton( const Spline::Point& point, double t0 ) const
{
	// minimiza la distancia entre el punto y el polinomio
	// d² = ||(x0,y0)-(sx(t),sy(t))||² = ||(x0-sx(t),y0-sy(t))||² = (x0-sx(t))² + (y0-sy(t))²
	// (d²)' = -2*(x0-sx(t))*sx'(t) - 2*(y0-sy(t))*sy'(t)
	// Busco t, tales que -2*(x0-sx(t))*sx'(t) - 2*(y0-sy(t))*sy'(t) = 0
	// Necesito (d²)'' = -2*(sx(t)-x0)*sx''(t) + 2*sx'(t)² - 2*(sy(t)-y0)*sy''(t) + 2*sy'(t)²

	// TODO: regular la cantidad de iteraciones (teóricamente)
	const uint N = 20; // cantidad máxima de iteraciones
	const double TOL = 1.0e-16; // tolerancia Newton
	double t = t0;
	
	forno( iteration, N )
	{
		double delta_x = (point.x-(*_sx)(t));
		double delta_y = (point.y-(*_sy)(t));

		double dsxt  = _sx->derivative(t);
		double dsyt  = _sy->derivative(t);
		double d2sxt = _sx->derivative2(t);
		double d2syt = _sy->derivative2(t);

		// f(t) = (d²)' [simplificadas]
		double ft  = -delta_x*dsxt - delta_y*dsyt;
		// f'(t) = (d²)''
		double dft = -delta_x*d2sxt + dsxt*dsxt
                     -delta_y*d2syt + dsyt*dsyt;

		double offset = ft/dft;

		// si está "muy cerca", quiere decir que lo encontró
		if( abs(offset) < TOL || std::isnan(offset) )
			break;

		t -= offset;		
	}

	return t;	
}

double SplineR2::nearest_point( const Spline::Point& point, uint i, double& min_dist ) const
{	
	const double lower_t = _cp_x[i].x;
	const double upper_t = _cp_x[i+1].x;

	// divide en 100 subintervalos
	const uint intervals = 100;

	// step = largo de cada subintervalo
	double step = (upper_t-lower_t)/(intervals-1);

	// inicializo con el más cercano de los puntos de control
	double min_t    = (distance(point, lower_t, true) < distance(point, upper_t, true))?lower_t:upper_t;
	min_dist = distance( point, min_t, true );

	double t = lower_t;

	// recorro cada subintervalo buscando ceros
	forsno( interval, 1, intervals )
	{
		double ddist = distanceDerivative(point, t);
		double next_ddist = distanceDerivative(point, lower_t+step*interval);

		// si la derivada cambia de signo se acerca a la raíz por bisección
		if( ddist*next_ddist < 0 )
			t = zeroWithBisection(point, t, t+step);
		// busca la raíz por Newton
		t = zeroWithNewton(point, t);

		// se queda con la menor de las distancias calculadas
		// (si está en el intervalo de los puntos de control)
		double dist = distance( point, t, true );
		if( (dist < min_dist) && (lower_t <= t && t < upper_t) )
		{
			min_dist = dist;
			min_t    = t;
		}

		// cambia al siguiente subintervalo
		t = lower_t+step*interval;
	}
	// devuelve el t del mínimo encontrado
	return min_t;
}

Spline::Point SplineR2::derivative( double t ) const
{
	return Spline::Point( _sx->derivative(t), _sy->derivative(t) );
}

Spline::Point SplineR2::derivative2( double t ) const
{
	return Spline::Point( _sx->derivative2(t), _sy->derivative2(t) );
}

void SplineR2::movePoint( double t, const Spline::Point& to )
{
	std::vector<Spline::Point>::iterator pos_x = _cp_x.begin(), pos_y = _cp_y.begin();

	// busca en qué intervalo está el t
	while( pos_x->x < t )
	{
		pos_x++;
		pos_y++;
	}

	if( pos_x->x != t )
	{
		// si no cae en un punto de control, lo crea
		_cp_x.insert( pos_x, Spline::Point(t,to.x) );
		_cp_y.insert( pos_y, Spline::Point(t,to.y) );
	}
	else
	{
		// si era un punto de control, lo mueve
		pos_x->y = to.x;
		pos_y->y = to.y;
	}

	// elimina el spline actual
	delete _sx;
	delete _sy;

	// calcula el nuevo spline
	_sx = new Spline( _cp_x );
	_sy = new Spline( _cp_y );
}

std::ostream& SplineR2::print( std::ostream& out ) const
{
	out << *_sx << std::endl;
	out << *_sy << std::endl;
}

std::ostream& operator<<( std::ostream& out, const SplineR2& s )
{
	s.print(out);
	return out;
}

